
#include "Vec3f.h"


////////////////////////////////////////////////////////////////////////////////
/*----------------------------------------------------------------------------*/
Vec3f::Vec3f() { 
	x = y = z = 0;
}

/*----------------------------------------------------------------------------*/
Vec3f::Vec3f(float px, float py, float pz) { 
	x = px; 
	y = py; 
	z = pz;
}

/*----------------------------------------------------------------------------*/
Vec3f::Vec3f(float xyz[3]) { 
	x = xyz[0]; 
	y = xyz[1]; 
	z = xyz[2];
}

/*----------------------------------------------------------------------------*/
Vec3f::Vec3f(const Vec3f& v) { 
	x = v.x; 
	y = v.y; 
	z = v.z;
}

/*----------------------------------------------------------------------------*/
Vec3f::Vec3f(int type) { 	
	if ( type == Random ) { (*this).randomize(); }
}


////////////////////////////////////////////////////////////////////////////////
/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator+=(const Vec3f& a) { 
	x += a.x; 
	y += a.y; 
	z += a.z;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator-=(const Vec3f& s) { 
	x -= s.x; 
	y -= s.y; 
	z -= s.z;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator*=(const Vec3f& s) { 
	x *= s.x; 
	y *= s.y; 
	z *= s.z;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator/=(const Vec3f& d) { 
	x /= d.x; 
	y /= d.y; 
	z /= d.z;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator*=(float s) { 
	x *= s; 
	y *= s; 
	z *= s;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator/=(float d) { 
	float s = 1/d; 
	x *= s; 
	y *= s; 
	z *= s;
	return (*this);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator=(const Vec3f& v) { 
	x = v.x; 
	y = v.y; 
	z = v.z; 
	return *this;
}

/*----------------------------------------------------------------------------*/
bool Vec3f::operator==(const Vec3f& v) const { 
	return (x == v.x && y == v.y && z == v.z);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator+(const Vec3f& a) const { 
	return Vec3f(x+a.x, y+a.y, z+a.z);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator-(const Vec3f& s) const { 
	return Vec3f(x-s.x, y-s.y, z-s.z);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator-() const { 
	return Vec3f(-x, -y, -z);
}

/*----------------------------------------------------------------------------*/
float Vec3f::operator*(const Vec3f& d) const { // Dot Product
	return x*d.x + y*d.y + z*d.z; 
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator*(float s) const { 
	return Vec3f(x*s, y*s, z*s);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::operator/(float d) const {
	float s = 1/d; 
	return Vec3f(x*s, y*s, z*d);
}

/*----------------------------------------------------------------------------*/
float Vec3f::operator[](int i) const {
	switch ( i ) {
		case 0: return x;
		case 1: return y;
		case 2: return z;
		default: return 0;
	}
}

/*----------------------------------------------------------------------------* /
ostream& Vec3f::operator<<(ostream& os) const {
	os << "Vec3f( \t" << x << ", \t" << y << ", \t" << z << " )";
	return os;
}*/

/*----------------------------------------------------------------------------*/
void Vec3f::print() const {
	cout << "Vec3f( \t" << x << ", \t" << y << ", \t" << z << " )" << endl;
}


////////////////////////////////////////////////////////////////////////////////
/*----------------------------------------------------------------------------*/
float Vec3f::mag() const { return x*x + y*y + z*z; }

/*----------------------------------------------------------------------------*/
float Vec3f::length() const { return sqrt(x*x + y*y + z*z); }

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::normalized() const {
	float len = length();
	if ( !len ) { return Vec3f(); }
	return ((*this)/len);
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::crossedWith(const Vec3f& v) const {
	return Vec3f( 
		(y*v.z)-(z*v.y), 
		(z*v.x)-(x*v.z), 
		(x*v.y)-(y*v.x)
	);
}

/*----------------------------------------------------------------------------*/
// This requires/assumes that x, y, and z are allocated together in memory
const float* Vec3f::getArray() const { return &x; }


////////////////////////////////////////////////////////////////////////////////
/*----------------------------------------------------------------------------*/
void Vec3f::normalize() {
	float len = length();
	if ( !len ) { return; }
	(*this) /= len;
}

/*----------------------------------------------------------------------------*/
void Vec3f::normalizeAtLength(float currLen) {
	if ( !currLen ) { return; }
	(*this) /= currLen;
}

/*----------------------------------------------------------------------------*/
void Vec3f::normalizeToLength(float newLen) {
	float len = length();
	if ( !len ) { return; }
	(*this) *= (newLen/len);
}

/*----------------------------------------------------------------------------*/
void Vec3f::randomize() {
	float halfMax = RAND_MAX >> 1;
	x = rand()-halfMax;
	y = rand()-halfMax;
	z = rand()-halfMax;
	this->normalize();
}

/*----------------------------------------------------------------------------*/
Vec3f Vec3f::multiplyBy4x4Matrix(const float* m) {

	Vec3f v = Vec3f(
		(x*m[ 0] + y*m[ 1] + z*m[ 2] + m[ 3]),
		(x*m[ 4] + y*m[ 5] + z*m[ 6] + m[ 7]),
		(x*m[ 8] + y*m[ 9] + z*m[10] + m[11])
	);

	x = v.x;
	y = v.y;
	z = v.z;

	return v;
}






/*
bool onLineSeg(Vec3f p, Vec3f a, Vec3f b, Vec3f v) {

	Vec3f c = subVec(b, a);

	// check cross product step by step for speed
	if ( !nearZero( (c.y*v.z)-(c.z*v.y) ) ) { return false; }
	if ( !nearZero( (c.z*v.x)-(c.x*v.z) ) ) { return false; }
	if ( !nearZero( (c.x*v.y)-(c.y*v.x) ) ) { return false; }
	
	float dist1 = len(subVec(p,a)) + len(subVec(p,b));
	float dist2 = len(subVec(a,b));
	return nearZero(dist1 - dist2);
}

bool pointInTri(Vec3f p, Vec3f v0, Vec3f v1, Vec3f v2, Vec3f fn, float dist, float rad, float epsilon) {
	
	float push = dist < rad ? dist : rad;
	p = subVec(p, scaleVec(fn,push));

    Vec3f vec0 = subVec(p,v0);
    Vec3f vec1 = subVec(p,v1);
    Vec3f vec2 = subVec(p,v2);

	vec0 = normalize(vec0);
	vec1 = normalize(vec1);
	vec2 = normalize(vec2);

    float angle =	acos(dProd(vec0,vec1))+ 
					acos(dProd(vec1,vec2))+ 
					acos(dProd(vec2,vec0));
 
	if ( abs( angle - TWO_PI ) < epsilon ) { 
		return true; }

	if ( onLineSeg(p, v0, v1, vec0) || 
		 onLineSeg(p, v1, v2, vec1) ||
		 onLineSeg(p, v2, v0, vec2) ) { 
		return true;
	}

	return false;
}

bool sameSide(Vec3f p1, Vec3f p2, Vec3f a, Vec3f b) {
	Vec3f cp1 = cProd(subVec(b,a),subVec(p1,a));
	Vec3f cp2 = cProd(subVec(b,a),subVec(p2,a));
	if ( dProd(cp1,cp2) >= 0 ) { return true; }
	return false;
}

bool pointInTriCP(Vec3f p, Vec3f a, Vec3f b, Vec3f c) {
	if ( sameSide(p,a,b,c) && sameSide(p,b,a,c) && sameSide(p,c,a,b) ) {
		return true;
	}
	return false;
}
*/
